#include <mml/polygon.h>
#include <mml/primitives.h>



/** Flatten a polygon. */
void mml::flattenPolygon(int n,
                         double* vertexData,
                         double normal[3],
                         double centroid[3],
                         double& area, double& projectedArea)
{
  // Get normal, fake centroid and area:
  double fakeCentroid[3];
  analysePolygon(n, vertexData, normal, fakeCentroid, area, projectedArea);

  if (area > 0.0) {
    // Determine the real centroid:
    vector_reset(3, centroid);
    for (int i = 0; i < n; ++i) {
      double const* p1 = &vertexData[3 * i];
      double const* p2 = &vertexData[3 * ((i + 1) % n)];

      // p1 - centroid
      double diff1[3];
      vector_copy(3, diff1, p1);
      vector_subtract(3, diff1, fakeCentroid);

      // p2 - centroid
      double diff2[3];
      vector_copy(3, diff2, p2);
      vector_subtract(3, diff2, fakeCentroid);

      // (p1 - fakeCentroid) x (p2 - fakeCentroid):
      double localNormal[3];
      vector_cross(diff1, diff2, localNormal);

      double weight = vector_ip(3, localNormal, normal);
      vector_add(3, centroid, p1, weight);
      vector_add(3, centroid, p2, weight);
    }
    vector_scale(3, centroid, 0.25 / area);

    // Flatten the polygon:
    double centroidDistance = vector_ip(3, centroid, normal);
    for (int i = 0; i < n; ++i) {
      double* v = &vertexData[3 * i];  // vertex i

      // Nullify distance along normal from vertex i to centroid:
      vector_add(3, v, normal, centroidDistance - vector_ip(3, v, normal));
    }
  }
  else {
    vector_copy(3, centroid, fakeCentroid);
  }
}
